/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import br.ufrn.engcomp.mpc.Constants;
import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

/**
 *
 * @author Felipe
 */
public abstract class AbstractCoupledTanksPlant implements Plant {
    
    protected static final double LIMITE_ATUADOR = 3;
    protected static final double LIMITE_SUPERIOR_TANQUES = 27;
    protected static final double LIMITE_INFERIOR_TANQUES = 3;
    
    protected RealMatrix aMatrix;
    protected RealMatrix bMatrix;
    protected RealMatrix cMatrix;
    protected double[] inputOP;
    protected double[] stateOP;
//    protected Constraints constraints;
    
    public AbstractCoupledTanksPlant() {
        // Values of linearization of the system
        aMatrix = new Array2DRowRealMatrix(Constants.N, Constants.N);
        bMatrix = new Array2DRowRealMatrix(Constants.N, Constants.L);
        cMatrix = new Array2DRowRealMatrix(Constants.M, Constants.N);
        
//        // Para Ts = 0.5s V1
//        aMatrix.setEntry(0, 0, 0.9677);
//        aMatrix.setEntry(0, 1, 0);
//        aMatrix.setEntry(1, 0, 0.03174);
//        aMatrix.setEntry(1, 1, 0.9677);
//        bMatrix.setEntry(0, 0, 0.1458);
//        bMatrix.setEntry(1, 0, 0.002378);
        
        // Para Ts = 0.5s V2
        aMatrix.setEntry(0, 0, 0.9677);
        aMatrix.setEntry(0, 1, 0);
        aMatrix.setEntry(1, 0, 0.03176);
        aMatrix.setEntry(1, 1, 0.9677);
        bMatrix.setEntry(0, 0,  0.729);
        bMatrix.setEntry(1, 0, 0.0119);
        
//        // Para Ts = 0.1s
//        aMatrix.setEntry(0, 0, 0.9935);
//        aMatrix.setEntry(0, 1, 0);
//        aMatrix.setEntry(1, 0, 0.00652);
//        aMatrix.setEntry(1, 1, 0.9935);
//        bMatrix.setEntry(0, 0, 0.1477);
//        bMatrix.setEntry(1, 0, 0.0004842);
        
//        // Para Ts = 0.2s
//        aMatrix.setEntry(0, 0, 0.987);
//        aMatrix.setEntry(0, 1, 0);
//        aMatrix.setEntry(1, 0, 0.01295);
//        aMatrix.setEntry(1, 1, 0.987);
//        bMatrix.setEntry(0, 0, 0.2945);
//        bMatrix.setEntry(1, 0, 0.001929);
        
//        // Para Ts = 0.3s
//        aMatrix.setEntry(0, 0, 0.9805);
//        aMatrix.setEntry(0, 1, 0);
//        aMatrix.setEntry(1, 0, 0.01931);
//        aMatrix.setEntry(1, 1, 0.9805);
//        bMatrix.setEntry(0, 0, 0.4403);
//        bMatrix.setEntry(1, 0, 0.00432);
        
//        // Para Ts = 0.4s
//        aMatrix.setEntry(0, 0, 0.9741);
//        aMatrix.setEntry(0, 1, 0);
//        aMatrix.setEntry(1, 0, 0.02557);
//        aMatrix.setEntry(1, 1, 0.9741);
//        bMatrix.setEntry(0, 0, 0.5851);
//        bMatrix.setEntry(1, 0, 0.007647);
        
        cMatrix.setEntry(0, 0, 0);
        cMatrix.setEntry(0, 1, 1);
    }

    @Override
    public RealMatrix getAMatrix() {
        return aMatrix;
    }

    @Override
    public void setAMatrix(RealMatrix A) {
        this.aMatrix = A;
    }

    @Override
    public RealMatrix getBMatrix() {
        return bMatrix;
    }

    @Override
    public void setBMatrix(RealMatrix B) {
        this.bMatrix = B;
    }

    @Override
    public RealMatrix getCMatrix() {
        return cMatrix;
    }

    @Override
    public void setCMatrix(RealMatrix C) {
        this.cMatrix = C;
    }

    /**
     * @return the inputOP
     */
    @Override
    public double[] getInputOP() {
        return inputOP;
    }

    /**
     * @param inputOP the inputOP to set
     */
    @Override
    public void setInputOP(double[] inputOP) {
        this.inputOP = inputOP;
    }

    /**
     * @return the stateOP
     */
    @Override
    public double[] getStateOP() {
        return stateOP;
    }

    /**
     * @param stateOP the stateOP to set
     */
    @Override
    public void setStateOP(double[] stateOP) {
        this.stateOP = stateOP;
    }

//    /**
//     * @return the constraints
//     */
//    @Override
//    public Constraints getConstraints() {
//        return constraints;
//    }
//
//    /**
//     * @param constraints the constraints to set
//     */
//    @Override
//    public void setConstraints(Constraints constraints) {
//        this.constraints = constraints;
//    }

    @Override
    public boolean isConstrained() {
//        return constraints != null;
        return this instanceof PlantWithConstraints;
    }
    
    // Calcula x(k+1) = Ax(k) + Bu(k)
    @Override
    public double[] computeXModel(double[] lastX, double[] lastU) {
        RealMatrix lastXVector = new Array2DRowRealMatrix(lastX);
        RealMatrix lastUVector = new Array2DRowRealMatrix(lastU);

        return MathCommonsUtils.convertMatrixToColumnVector(aMatrix.multiply(lastXVector).add(bMatrix.multiply(lastUVector))).getData();
    }

    // Calcula y(k) = Cx(k)
    // é pra receber lastX ou X atual???
    @Override
    public double[] computeYModel(double[] lastX) {
        RealMatrix lastXVector = new Array2DRowRealMatrix(lastX);
        
//        return MathCommonsUtils.convertMatrixToColumnVector(cMatrix.multiply(lastXVector)).getData();
        return lastX;
    }
    
    protected double[] computeLowerLimitValue(double operationPoint, double lowerLimit) {
        double[] limitValue = new double[2];
        
        double lowerTerm = (lowerLimit - operationPoint) * -1;

        if (lowerTerm > 0) {
            limitValue[0] = -1 / Math.abs(lowerTerm);
            limitValue[1] = -1;
        } else if (lowerTerm < 0) {
            limitValue[0] = -1 / Math.abs(lowerTerm);
            limitValue[1] = 1;
        } else {
            limitValue[0] = -1;
            limitValue[1] = 0;
        }
        
        return limitValue;
    }
    
    protected double[] computeUpperLimitValue(double operationPoint, double upperLimit) {
        double[] limitValue = new double[2];
        
        double upperTerm = upperLimit - operationPoint;

        if (upperTerm > 0) {
            limitValue[0] = 1 / Math.abs(upperTerm);
            limitValue[1] = -1;
        } else if (upperTerm < 0) {
            limitValue[0] = 1 / Math.abs(upperTerm);
            limitValue[1] = 1;
        } else {
            limitValue[0] = 1;
            limitValue[1] = 0;
        }
        
        return limitValue;
    }
    
}
